package math;

import java.util.ArrayList;
import java.util.List;

import javax.media.j3d.Transform3D;
import javax.vecmath.AxisAngle4d;
import javax.vecmath.Vector3d;

//import static org.junit.Assert.*;

public class Vectors {
	
	public static Double findAngle(Vector3d alignVector, Vector3d reference,
			double angleFromReference, Vector3d axis) {
		reference.normalize();
		axis.normalize();
		alignVector.normalize();
		List<Double> posCandidates = new ArrayList<Double>();
		List<Double> negCandidates = new ArrayList<Double>();

		Vector3d findPos = new Vector3d(alignVector);
		Vector3d findNeg = new Vector3d(alignVector);
		
		Transform3D transformPos = new Transform3D();
		Transform3D transformNeg = new Transform3D();
		double step = Precision.ERROR_MARGIN/5;
		AxisAngle4d axisAngle = new AxisAngle4d(axis, step);
		transformPos.set(axisAngle);
		axisAngle = new AxisAngle4d(axis, -step);
		transformNeg.set(axisAngle);
		for (double d=0; d<=Math.PI; d+=step) {
			if (Precision.compare(reference.angle(findPos), angleFromReference)) {
				posCandidates.add(d);
			} else if (posCandidates.size() > 0) {
				return returnAverage(posCandidates);
			}
			if (Precision.compare(reference.angle(findNeg), angleFromReference)) {
				negCandidates.add(-d);
			} else if (negCandidates.size() > 0) {
				return returnAverage(negCandidates);
			}
			transformPos.transform(findPos);
			transformNeg.transform(findNeg);
//			assertEquals(angleFromReference, whatWeWant.angle(reference), Precision.ERROR_MARGIN);
			
		}
		
		return null;
	}
	
	private static double returnAverage(List<Double> list) {
		double sum = 0;
		for (Double a : list) {
			sum += a;
		}
		return sum / list.size();
	}
	
}
